#pragma once
#include "AStar2D.h"

class AWA2D : public AStar2D
{
protected:
	double weightg; /* Weight of g value */
	double weighth; /* Weight of h value */
	char name[30];
	int bestDepth; /* The cost of the best goal found so far */
	
	
	virtual int cost(int h, int g)
	{
		return (int)(weightg*g+weighth*h);	
	}

	
	/**
	 * EXPAND takes the index of the best node on OPEN, unpacks it, generates each
	   of its children, evaluates those that aren't on OPEN or CLOSED, and inserts them
	   into OPEN. 
	   Main difference from simple A*: If a goal found, still generate all neighbors, since may lead to better paths.
	   (NOT SURE ABOUT THIS)
	   Parameters: index - hash table index of state being expanded
	*/
	virtual int expand(int index)
	{
		int blank;                                      /* current position of blank */
		unsigned int nps[WORDS];                                  /* new state in packed form */
		int opindex;                                    /* index into operator array */
		int newindex;                                /* hashtable index of new state */
		int newblank;                                 /* blank position in new state */
		int tile;                                                /* tile being moved */
		int newh;                                 /* static h values of child states */
		int newg;                                           /* g value of new states */
		int hashval;                                  /* the hash value of the state */

		expanded++;                                    /* count total nodes expanded */
		blank = unpackn(old[index].ps, s);  /* unpack state and return blank position */
		newg = old[index].g+1;     /* g value of children is one greater than parent */

		for (opindex = 0; opindex < oprs[blank].num; opindex++) /* for each appl opr */
			if ((newblank = oprs[blank].pos[opindex]) != old[index].oldblank) 
			{
				s[blank] = tile = s[newblank];   /* tile moved is in new blank position */
				s[newblank] = 0;                     /* put blank in new blank position */
				newh = old[index].h + manhat[tile][blank] - manhat[tile][newblank];
				if ((newh + newg < bestDepth) && (newh+newg<=goalUpperbound))		/* pruning states which previously solution is already better */
				{
					generated++;                     /* increment number of nodes generated */
					if (newh == 0 && newg < bestDepth)
						return (newg);  /* goal could be chosen */

					packn(s, nps);                                     /* pack state into ps */
					hashval = hash(nps);                    /* hash the state into a bucket */
					newindex = search(nps, hashval);       /* target location in hash table */

					if (newindex == -1){                     /* state was not found in table */
						//printf("Insert a new node g=%d, h=%d (%d)\n",newg,newh,empty);
						insert(nps, hashval, blank, newg, newh);     /* insert into open list */
					}
					else
						if (old[newindex].g > newg)      /* if not in table or longer path */
						{
							close(newindex);            /* remove node from old location on open */
							reopened++;             /* keep track of how many times this happens */
							insert(nps, hashval, blank, newg, newh);   /* insert into open list */
						}
				}

				s[newblank] = tile;          /* undo current move before doing the next */
				s[blank] = 0;                                 /* restore blank as well */
			}
		return(0);                                             /* no goal found yet */
	}


	int countOpenlistEntries();
public:

	AWA2D(double pWeightg, double pWeighth) : AStar2D()
	{
		weightg = pWeightg;
		weighth = pWeighth;

		if(pWeightg==0)
			sprintf(algorithmName,"GBFS2D");
		else
			sprintf(algorithmName,"AWA2D-%1.2f",(pWeighth/pWeightg));
		bestDepth = MAXG;

		maxCost = calculateMaxCost();
	}

	virtual ~AWA2D()
	{
	}


	/**
	 * Continues A* expansion cycle until a goal has been found
	 **/
	virtual int expandUntilFindGoal()
	{
		int index;  /* general utility index */
		depth=bestDepth;
		while (stored < FULL) /* expand until goal found or hash table full*/
		{			
			// If open list is empty - best depth is optimal solution
			if((bestf==maxCost) && (bestg==MAXG))
			{
				depth = bestDepth; // OPEN is empty - best depth is the optimal one
				return bestDepth;
			}
			
			index = open[bestf][bestg];                       /* new best state on open list */		
			depth = expand(index);          /* expand best state, generating children */
			close(index);                           /* move best state to closed list */

			// If goal has been found in this expansion
			if((depth>0) && (depth<bestDepth))
			{
				bestDepth=depth;
				pregoalIndex=index;
				break;
			}
			else
				depth=bestDepth;
		}

		return bestDepth;
	}

	/* CLOSE takes an index into the hash table and deletes it from the OPEN list,
	if its there.  
	Paramerter: int index -  index of offending element in hash table */
	virtual void close(int index)
	{
		closed++;

		int f;                                     /* f value of state to be deleted */
		int g;                                     /* g value of state to be deleted */ 
		int next;                                 /* pointer to next element in list */
		
		g=old[index].g;
		f = cost(old[index].h,g);      /* compute F value of state */
		
		if (index == open[f][g])                     /* if 1st element of its open list */
		{
			next = old[index].next;                    /* second element of open list */
			if(next!=MAX)
				old[next].prev = MAX;                        /* 2nd element will be first */
			open[f][g] = next;                          /* head points to second element */
		}
		else
		{	
			// If closing a node that is not in the top of OPEN, connect it prev and next
			if(old[index].prev!=MAX) 
			{		
				old[old[index].prev].next = old[index].next;         /* set previous to next */
				if (old[index].next != MAX) /* not 1st and no previous, closed node */
					old[old[index].next].prev = old[index].prev;        /* set next to previous */
			}
		}

		// Clear openlist pointers 
		old[index].next=MAX;
		old[index].prev=MAX;

		// Update bestf and bestg if this is the top node in OPEN
		if((bestf==f) && (bestg==g)){
			if(open[f][g]==MAX){
				// Need to search for the next best f and g
				// TODO: Implement better than linear search
				for(int newbestf=f;newbestf<maxCost;newbestf++){					
					for(int newbestg=g-1;newbestg>0;newbestg--){
						if(open[newbestf][newbestg]!=MAX){
							bestf=newbestf;
							bestg=newbestg;
							return;
						}
					}
					g=MAXG;
					fDiff++;		 /** increasing fDiff counter **/
				}
				// OPEN is empty and the search can halt with the bestDepth being the optimal solution
				bestf=maxCost; // These will serve as flags that all done
				bestg=MAXG;				
			}
		}
	}

	/**
	 * Return the quality of the found solution  
	 **/
	virtual double getSolutionQualityFactor()
	{
		// If OPEN is empty, then optimal solution has been found
		if(bestf==maxCost)
			return 1;

		return ((double)getLowerbound()/(double)bestDepth);
	}

	/**
	 * Return the quality of the found solution  
	 **/
	virtual int getLowerbound()
	{
		int next;
		int admissibleF;			/* storing currentf to save recomputation */
		int lowerbound=MAXG;

		// Run over all the open list and search for minimum f value
		for(int f=bestf;f<maxCost;f++){
			for(int g=0;g<MAXG;g++){
				next=open[f][g];
				while(next!=MAX){
					admissibleF = old[next].g + old[next].h;
					if ((old[next].g!= -1) && (admissibleF > 0) && (admissibleF < lowerbound))
						lowerbound = admissibleF;	/* if current node is valid and best, keep it */
					next=old[next].next;					
				}
			}
		}
		return lowerbound;
	}
};